#include "file_sl.hpp"

void Angle_Point_Cloud::clear(void)
{
  my_point.clear();
}
void Angle_Point_Cloud::add_point(pcl::PointXYZ &point_temp)
{
  My_Point mp;

  mp.x=point_temp.x;
  mp.y=point_temp.y;
  mp.z=0;
  mp.p_distance.resize(0);
  my_point.push_back(mp);
}
void Angle_Point_Cloud::add_point(My_Point &point_temp)
{
  
  point_temp.p_distance.resize(0);
  my_point.push_back(point_temp);
}
void Angle_Point_Cloud::add_point(pcl::PointXYZ &point_temp,double s_angle,double dis,int attr)
{
  My_Point mp;

  mp.x=point_temp.x;
  mp.y=point_temp.y;
  mp.z=0;
  mp.angle=s_angle;
  mp.distance = dis;
  mp.attribute = attr;
  mp.p_distance.resize(0);
  my_point.push_back(mp);
}
void Angle_Point_Cloud::set_input_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_)
{
    std::size_t size;
    size=cloud_->points.size();
    my_point.clear();
    if(size>0)
    {
        my_point.clear();
        for(int i=0;i<size;i++)
          add_point(cloud_->points[i]);
    }
}
void Angle_Point_Cloud::write_cloud(void)
{
  std::ofstream file(filename, std::ios::out); // "app"表示追加模式，避免覆盖已存在的内容
  if(file.is_open())
  {
    std::size_t size=my_point.size();
    file<<size<<std::endl;
    for(int i=0;i<size;i++)
      {
        file<<my_point[i].x<<" "<<my_point[i].y<<" "<<my_point[i].z<<std::endl;
        file<<"  "<<my_point[i].angle<<"  "<<my_point[i].distance<<"  "<<my_point[i].attribute<<std::endl;
      }
    file.close();
  }
   
}
void Angle_Point_Cloud::read_cloud(void)
{
    
    int value;
    double t[10];
    My_Point p;
    std::fstream file(filename,std::ios::in); // "app"表示追加模式，避免覆盖已存在的内容
    if(file.is_open())
    {
      my_point.clear();
      file>>value;
      for(int i=0;i<value;i++)
        {
          file>>p.x>>p.y>>p.z;
          file>>p.angle>>p.distance>>p.attribute;
          add_point(p);
        }
      file.close();
    }
}
void Angle_Point_Cloud::claculate_distance(void)
{
  if(my_point.size()<3)return;
   for (int i = 0; i < my_point.size(); i++)
    {
        for (int j = i+1; j < my_point.size(); j++)
         {
          double dx = my_point[j].x - my_point[i].x;
          double dy = my_point[j].y - my_point[i].y;
          double dist = sqrt(dx * dx + dy * dy);
          my_point[i].p_distance.push_back({j, dist}); // 存储点i到点j的距离
         // my_point[j].p_distance.push_back({i, dist});
        }
    }
}